#include <iostream>
#include <vector>
#include <ros/ros.h>
#include "find_path/pathSearch.h"
using namespace std;
int width,height;
double resolution;
int originx,originy;
//source devel/setup.bash
//roslaunch find_path findpath.launch
//catkin_make -DCATKIN_WHITELIST_PACKAGES="find_path"
int main(int argc, char **argv) {
    ros::init(argc, argv, "subscribe_node");
    ros::NodeHandle nh;
    nh.param<int>("/find_path/map_height", width, 1000);
    nh.param<int>("/find_path/map_height", height, 1000);
    nh.param<double>("/find_path/map_resolution", resolution, 0.5);
    nh.param<int>("/find_path/map_origin_x", originx, -500);
    nh.param<int>("/find_path/map_origin_y", originy, -500);
    Map a(resolution,width,height,originx,originy);
    ros::Publisher map_pub = nh.advertise<nav_msgs::OccupancyGrid>("/map", 1000);
    ros::Publisher path_pub = nh.advertise<nav_msgs::Path>("/path", 1000);
    a.setPublisher(map_pub,path_pub);
    aStarMethod model(&a);
    ros::Subscriber chatter_sub = nh.subscribe("/robot_pose_ekf/odom_combined", 1000, &Map::update,&a);
    ros::ServiceServer state_srv_ = nh.advertiseService("get_path", &aStarMethod::getPath, &model);
    ros::spin();
    return 0;
}
